% deltaGPS.m
% By Peter Horak for the GreenCube project (Dartmouth College)
%
% Purpose: Perform numerical differentiation on GPS flight data
%
% Input: dalt, dlat, dlng, dtime
% Output: deltaLat, deltaLng, deltaAlt, delta, ddLat, ddLng, ddAlt, ddelta

% dlatdeg = floor(dlat./100);
% dlatmin = mod(dlat,100);
% dlngdeg = floor(dlng./100);
% dlngmin = mod(dlng,100);
% lat = dlatdeg + dlatmin./60;
% lng = dlngdeg + dlngmin./60;

lat = (floor(dlat./100)+mod(dlat,100)./60).*364564.09;
lng = (floor(dlng./100)+mod(dlng,100)./60).*261520.05;

% lat = lat.*364564.09; %lat 364564.09 ft/deg
% lng = lng.*261520.05; %lng 261520.05 ft/deg

deltaTime = dtime(2:end)-dtime(1:(end-1));
deltaLat = (lat(2:end)-lat(1:(end-1)))./deltaTime;
deltaLng = (lng(2:end)-lng(1:(end-1)))./deltaTime;
deltaAlt = (dalt(2:end)-dalt(1:(end-1)))./deltaTime;
delta = sqrt(deltaAlt.^2+deltaLat.^2+deltaLng.^2);

ddLat = (deltaLat(2:end)-deltaLat(1:(end-1)))./deltaTime(2:end);
ddLng = (deltaLng(2:end)-deltaLng(1:(end-1)))./deltaTime(2:end);
ddAlt = (deltaAlt(2:end)-deltaAlt(1:(end-1)))./deltaTime(2:end);
ddelta = sqrt(ddAlt.^2+ddLat.^2+ddLng.^2);
% deltaT = diff(dtime);
% ddlat = diff(diff(lat)./diff(dtime))./deltaT(2:end);
% ddlng = diff(diff(lng)./diff(dtime))./deltaT(2:end);
% ddalt = diff(diff(dalt)./diff(dtime))./deltaT(2:end);
% ddelta = sqrt(ddalt.^2+ddlat.^2+ddlng.^2);

%plot(dtime(2:(end-1)),ddelta,dtime,sqrt((3.2808.*9.81*ax./1000).^2+(3.2808.*9.81*ay./1000).^2+(3.2808.*9.81*az./1000).^2))
%legend('|ddGPS|','|Acc|')
%xlabel('time (sec)')
%ylabel('ft/s^2')